#include <helper.h>
#include <ArgParser.hpp>
#include <ShowUtils.hpp>

#include <point_cloud/PCDOperator.hpp>
#include <trajectory/TrajOperator.hpp>

#include <SemanticCloud.hpp>
#include <PlaneMap.hpp>
#include <ExtractPlanes.hpp>
#include <LineExtractor.hpp>

using namespace std;
using namespace Eigen;

void visPlane(const ShowUtils &su, const Plane& plane, std::string label, int cnt, 
	std::string show_field = "custom", bool only_show_name = false)
{
	Vector4d coeff(plane.getA(), plane.getB(), plane.getC(), -plane.getD());
	su.ShowPlane(coeff, plane.getMeanPoint(), label+to_string(cnt), only_show_name);
	
	// 待优化
	su.ShowCloud(plane.getPointCloud(), cnt, "cloud_"+label, show_field, 2);
}

int main(int argc, const char **argv) 
{
    ArgumentParser parser;
	parser.addArgument("-i", "--input_dir", true);
	parser.addArgument("-t", "--traj_path", true);
	parser.addArgument("-o", "--output_dir", true);
    parser.addArgument("-b", "--begin_id", true);
    parser.addArgument("-e", "--end_id", true);
    parser.parse(argc, argv);

    string input = parser.get("input_dir");
	string traj_path = parser.get("traj_path");
	string output = parser.get("output_dir");
    int begin = parser.get<int>("begin_id");
	int end = parser.get<int>("end_id"); 
    int frame_id = begin;
    makeDir(output);

    PCDReader reader(input);
    TrajIO traj(traj_path);

    PointCloud::Ptr cloud(new PointCloud);
    SemanticCloud scloud;
	PlaneMapper plmap;
	plmap.setOutDir(output);

	ShowUtils su("Show Framework", false);
	su.setBackGroundColor(0.0, 0.0, 0.0);
	su.ShowPose(Matrix4d::Identity());

    PlaneUtils pu;

    // 开始读取
	while(reader.readPointCloud(cloud, frame_id))
    { 
        scloud.fromPointCloud(cloud);
		
		Plane floor_plane;
		Plane ceiling_plane;
		// 单帧中的平面
		vector<Plane> wall_planes;

		// 提取墙面
		std::vector<PointCloud::Ptr> segments;

		// 提取天花板
		if(scloud.ceiling->points.size() > MIN_POINTS_IN_PLANE)
		{
			ceiling_plane = extractPlane(scloud.ceiling);
			ceiling_plane.setFrameId(frame_id);
			ceiling_plane.setLabel(scloud.label_map["ceiling"]);
			wall_planes.push_back(ceiling_plane);
		}

        // 提取地面
		if(scloud.floor->points.size() > MIN_POINTS_IN_PLANE) // 地面点数不能太少
		{
			floor_plane = extractPlane(scloud.floor);
			floor_plane.setFrameId(frame_id);
			floor_plane.setLabel(scloud.label_map["floor"]);
			wall_planes.push_back(floor_plane);
		}

		// 区域生长分割
		segmentWall(scloud.wall, segments);

		for(int i=0; i<segments.size(); ++i)
		{
			Plane wall_plane = extractPlane(segments[i]);

			// 希望根据墙面的大小，调整平面提取的参数
			// 只会影响到reCompute函数的提取结果
			wall_plane.setPercentOfWall( segments[i]->points.size() / (float)scloud.wall->points.size() );
			wall_plane.setFrameId(frame_id);
			wall_plane.setLabel(scloud.label_map["wall"]);
			wall_planes.push_back(wall_plane);
		}

		pu.removeDuplicate(wall_planes);

        //if(frame_id == begin) line.initialize(ceiling_plane, floor_plane);
		//else line.processWallPlanes(plmap.getWallPlanes());

		Matrix4d pose = traj.getPoseMatrix(frame_id);
		
		// 可视化位姿
		su.ShowPose(pose, frame_id);

		// 发生更新的平面，和新平面
		// 剩下的是不发生改变的平面，以及被直接忽略的无效平面）
		vector<int> update_idx, new_idx; 
		
		std::cout << "Run Plane Mapper..." << std::endl;
		plmap.processPlane(ceiling_plane, pose);
		plmap.processPlane(floor_plane, pose);
		plmap.processWallPlanes(wall_planes, pose, update_idx, new_idx);

        #if SHOW_PLANE
        visPlane(su, plmap.getCeilingPlanes()[0], "Ceiling", 0, "custom");
		visPlane(su, plmap.getFloorPlanes()[0], "Floor", 0, "custom");
		for(int i=0; i<plmap.getWallPlanes().size(); ++i)
			visPlane(su, plmap.getWallPlanes()[i], "Wall", i, "custom");
        #endif

        LineExtractor line;
	    line.setOutDir(output);
        line.setCeilingPlane(plmap.getCeilingPlanes()[0]);
        line.setFloorPlane(plmap.getFloorPlanes()[0]);
        line.processWallPlanes(plmap.getWallPlanes());

        for(int i=0; i<line.getWallNum(); ++i) // 单帧内多少墙
		{
			auto& framework = line.getFramework(i);
			//std::cout << "wall" << i << " has lines:" <<  framework.size() << std::endl;
			for(int j=0; j<framework.size(); ++j)
			{
				//Transform<float, 3, Affine> tf(pose.cast<float>());
				auto start = framework[j].first.first; 
				auto end = framework[j].first.second; 
				//std::cout << "line" << j << " label:" <<  framework[j].second << std::endl;
				//auto start = pcl::transformPoint(framework[j].first.first, tf);
				//auto end = pcl::transformPoint(framework[j].first.second, tf);
				su.ShowLine(start, end,
					"wall"+std::to_string(i)+"_line"+std::to_string(j), 
                    //"frame"+std::to_string(frame_id)+"_wall"+std::to_string(i)+"_line"+std::to_string(j), 
					framework[j].second, 4);
			}
		}

        std::cout << "frame_id: " << frame_id << std::endl;
		
		if(new_idx.size()>0)
			su.waitSpace();

        frame_id += traj.getFrameGap();
		if(frame_id > end) break;
    }

	su.waitSpace();
    return 0;
}
